import cv2
import glob
import numpy as np
import matplotlib.pyplot as plt
from moviepy.editor import VideoFileClip
from IPython.display import HTML
%matplotlib inline
# Make a list of calibration images and images path
doub_images = list(map(lambda imgName: (imgName, cv2.cvtColor(cv2.imread(imgName), cv2.COLOR_BGR2RGB)),
glob.glob('./camera_cal/*.jpg')))
fig, axes = plt.subplots(4, 5, figsize=(25, 12)) # 4 * 5 image architecture
# show 20 images and path name
for index, ax in zip(range(4*5), axes.flat):
if index < len(doub_images):
imgPath, image = doub_images[index]
ax.set_title(imgPath)
ax.imshow(image)
Build the show image function:
def showimg(img1, img2, name_img1, name_img2, x, y, img3=[], lbl3=[], cmap=None, n = 2):
f, (ax1, ax2) = plt.subplots(1, n, figsize=(x,y))
ax1.imshow(img1, cmap=cmap)
ax1.set_title(name_img1, fontsize=30)
ax2.imshow(img2, cmap=cmap)
ax2.set_title(name_img2, fontsize=30)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
We have 20 images of a chessboard, OpenCV functions provides cv2. FindChessboardCorners () and cv2. CalibrateCamera calibration () for image calibration.
cv2.findChessboardCorners(): The function attempts to determine whether the input image is a view of the chessboard pattern and locate the internal chessboard corners.
cv2.calibrateCamera(): The function estimates the intrinsic camera parameters and extrinsic parameters for each of the views.
def calibrate_camera():
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((9*6, 3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1, 2)
# Make a list of calibration images
cal_images = glob.glob('camera_cal/calibration*.jpg')
# Step through the list and search for chessboard corners
for fname in cal_images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
# If found, add object points, image points
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
return mtx, dist
mtx, dist = calibrate_camera()
cv2.undistort:The function transforms an image to compensate radial and tangential lens distortion.
test1 = cv2.imread('camera_cal/calibration1.jpg')
undist = cv2.undistort(test1, mtx, dist, None, mtx)
showimg(test1, undist, 'Original image', 'Distortion corrected image', 15, 4)
test5 = cv2.cvtColor(cv2.imread('test_images/test5.jpg'), cv2.COLOR_BGR2RGB)
undist_test5 = cv2.undistort(test5, mtx, dist, None, mtx)
showimg(test5, undist_test5, 'Original image', 'Distortion corrected image', 15, 4)
# %matplotlib qt
# source image points
plt.imshow(undist_test5)
plt.plot(753, 478, '.') # right_top
plt.plot(1075, 670, '.') # right_bottom
plt.plot(216, 688, '.') # left_bottom
plt.plot(549, 483, '.') # left_top
def warp(img):
# Define calibration box in source (original) and destination (desired or warped) coordinates
img_size = (img.shape[1], img.shape[0])
# # Another ways.
# src = np.float32([[(img_size[0] / 2) - 55, img_size[1] / 2 + 100],
# [((img_size[0] / 6) - 10), img_size[1]],
# [(img_size[0] * 5 / 6) + 60, img_size[1]],
# [(img_size[0] / 2 + 55), img_size[1] / 2 + 100]])
# dst = np.float32([[(img_size[0] / 4), 0],
# [(img_size[0] / 4), img_size[1]],
# [(img_size[0] * 3 / 4), img_size[1]],
# [(img_size[0] * 3 / 4), 0]])
# Four source coordinates
src = np.float32([[753, 478],
[1092, 670],
[216, 688],
[549, 483]])
# Four desired coordinates
dst = np.float32([[997, 1],
[982, 712],
[316, 712],
[366, 1]])
# Compute the perspective transform , M
M = cv2.getPerspectiveTransform(src, dst)
# Could compute the inverse also by swapping the input parameters
Minv = cv2.getPerspectiveTransform(dst, src)
# Create warped image - uses linear interpolation
warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)
return warped, M, Minv
%matplotlib inline
# Used perspective transform all test_images
test_images = glob.glob('test_images/*.jpg')
for i in range(len(test_images)):
test_image = cv2.cvtColor(cv2.imread(test_images[i]), cv2.COLOR_RGB2BGR)
undist_test = cv2.undistort(test_image, mtx, dist, None, mtx)
warped_undist_test, M, Minv = warp(undist_test)
showimg(undist_test, warped_undist_test, 'Original image', 'Warped image', 15, 4)
test4 = cv2.cvtColor(cv2.imread(test_images[6]), cv2.COLOR_RGB2BGR)
undist_test4 = cv2.undistort(test4, mtx, dist, None, mtx)
warped_undist_test4 = warp(undist_test4)[0]
plt.imshow(warped_undist_test4)
Now let's explore four color Spaces: RGB, HSV, HLS and LAB to detect the lane lines in the image.
RGB is red-green-blue color space. You can think of this as a 3D space, in this case a cube, where any color can be represented by a 3D coordinate of R, G, and B values. For example, white has the coordinate (255, 255, 255), which has the maximum value for red, green, and blue.
warped_undist_test4_RGB = warped_undist_test4
warped_undist_test4_RGB_R = warped_undist_test4_RGB[:,:,0]
warped_undist_test4_RGB_G = warped_undist_test4_RGB[:,:,1]
warped_undist_test4_RGB_B = warped_undist_test4_RGB[:,:,2]
showimg(warped_undist_test4_RGB, warped_undist_test4_RGB_R, 'Original RGB image', 'R-Channel', 15, 4)
showimg(warped_undist_test4_RGB_G, warped_undist_test4_RGB_B, 'G-Channel', 'B-Channel', 15, 4)
HSV (Hue, Saturation, Value):The HSV representation models the way paints of different colors mix together, with the saturation dimension resembling various shades of brightly colored paint, and the value dimension resembling the mixture of those paints with varying amounts of black or white paint.
warped_undist_test4_HSV = cv2.cvtColor(warped_undist_test4_RGB, cv2.COLOR_RGB2HSV)
warped_undist_test4_HSV_H = warped_undist_test4_HSV[:,:,0]
warped_undist_test4_HSV_S = warped_undist_test4_HSV[:,:,1]
warped_undist_test4_HSV_V = warped_undist_test4_HSV[:,:,2]
showimg(warped_undist_test4_HSV, warped_undist_test4_HSV_H, 'Original HSV image', 'H-Channel', 15, 4)
showimg(warped_undist_test4_HSV_S, warped_undist_test4_HSV_V, 'S-Channel', 'V-Channel', 15, 4)
HSL (Hue, Saturation, Lightness):The HSL model attempts to resemble more perceptual color models such as NCS or Munsell, placing fully saturated colors around a circle at a lightness value of 1/2, where a lightness value of 0 or 1 is fully black or white, respectively.
warped_undist_test4_HLS = cv2.cvtColor(warped_undist_test4_RGB, cv2.COLOR_RGB2HLS)
warped_undist_test4_HLS_H = warped_undist_test4_HLS[:,:,0]
warped_undist_test4_HLS_L = warped_undist_test4_HLS[:,:,1]
warped_undist_test4_HLS_S = warped_undist_test4_HLS[:,:,2]
showimg(warped_undist_test4_HLS, warped_undist_test4_HLS_H, 'Original HLS image', 'H-Channel', 15, 4)
showimg(warped_undist_test4_HLS_L, warped_undist_test4_HLS_S, 'L-Channel', 'S-Channel', 15, 4)
The Lab color space describes mathematically all perceivable colors in the three dimensions L for lightness and a and b for the color opponents green–red and blue–yellow.
warped_undist_test4_LAB = cv2.cvtColor(warped_undist_test4_RGB, cv2.COLOR_RGB2LAB)
warped_undist_test4_LAB_L = warped_undist_test4_LAB[:,:,0]
warped_undist_test4_LAB_A = warped_undist_test4_LAB[:,:,1]
warped_undist_test4_LAB_B = warped_undist_test4_LAB[:,:,2]
showimg(warped_undist_test4_LAB, warped_undist_test4_LAB_L, 'Original LAB image', 'LAB L-Channel', 15, 4)
showimg(warped_undist_test4_LAB_A, warped_undist_test4_LAB_B, 'LAB A-Channel', 'LAB B-Channel', 15, 4)
From the above diagram, the three color channels of LAB can be seen that the left lane line of the B channel is clearly indicated, while the lane line on the right side of the L channel in HLS is more obvious.We'll apply LAB B-threshold and HLS L-threshold to the image to highlight the lane lines.
def lab_b_channel(img):
# 1) Apply a threshold to the b channel
img = img[:, :, 2]
if np.max(img) > 180:
img = img*(255/np.max(img))
b_thresh_min = 190
b_thresh_max = 255
# 2) Return a binary image of threshold result
binary_output = np.zeros_like(img)
binary_output[(img > b_thresh_min) & (img <= b_thresh_max)] = 1
return binary_output
lab_b_channel_test4 = lab_b_channel(warped_undist_test4_LAB)
showimg(warped_undist_test4_LAB, lab_b_channel_test4, 'LAB image', 'B-thresholded LAB image', 15, 4, cmap = 'gray')
def hls_l_channel(img):
# 1) Apply a threshold to the S channel
l_img = img[:,:,1]
l_img = l_img*(255/np.max(l_img))
l_thresh_min = 210
l_thresh_max = 255
# 2) Return a binary image of threshold result
binary_output = np.zeros_like(l_img)
binary_output[(l_img > l_thresh_min) & (l_img <= l_thresh_max)] = 1
return binary_output
hls_l_channel_test4 = hls_l_channel(warped_undist_test4_HLS)
showimg(warped_undist_test4_HLS, hls_l_channel_test4, 'HLS_L image', 'L-thresholded HLS image', 15, 4, cmap = 'gray')
def combined_threshold(warped_undist_image):
# Convert to HVS color space
img_LAB = cv2.cvtColor(warped_undist_image, cv2.COLOR_RGB2LAB)
# Convert to HLS color space
img_HLS = cv2.cvtColor(warped_undist_image, cv2.COLOR_RGB2HLS)
# Apply a threshold to the s channel
img_b_LAB = lab_b_channel(img_LAB)
# Apply a threshold to the l channel
img_l_HLS = hls_l_channel(img_HLS)
# Threshold x gradient
combined_img = np.zeros_like(img_l_HLS)
combined_img[(img_l_HLS == 1)|(img_b_LAB == 1)] = 1
return combined_img
threshold_color_img = combined_threshold(warped_undist_test4)
showimg(warped_undist_test4, threshold_color_img, 'RGB image', 'Thresholded image', 15, 4, cmap = 'gray')
The effect is very good. In addition to using color channel to determine the threshold, Sobel can also be used, but there will be more noise after Sobel processing, so only color channel is used here.
def pipeline_process(img):
# Undistort
undistort_img = cv2.undistort(img, mtx, dist, None, mtx)
# Perspective Transform
warp_undist_img, M, Minv = warp(undistort_img)
# Create a thresholded binary image
thresh_img = combined_threshold(warp_undist_img)
return thresh_img, Minv
test_images_i = []
test_images_processed = []
test_images = glob.glob('test_images/*.jpg')
for test_image in test_images:
or_img = cv2.cvtColor(cv2.imread(test_image), cv2.COLOR_BGR2RGB)
test_images_i.append(or_img)
image, Minv = pipeline_process(or_img)
test_images_processed.append(image)
showimg(or_img, image, 'Original image', 'pipeline image', 15, 4, cmap='gray')
I first take a histogram along all the columns in the lower half of the image like this:
histogram = np.sum(threshold_color_img[threshold_color_img.shape[0]//2:,:], axis=0)
plt.plot(histogram)
With this histogram I am adding up the pixel values along each column in the image. In my thresholded binary image, pixels are either 0 or 1, so the two most prominent peaks in this histogram will be good indicators of the x-position of the base of the lane lines. I can use that as a starting point for where to search for the lines. From that point, I can use a sliding window, placed around the line centers, to find and follow the lines up to the top of the frame.
def sliding_window(img):
"""
Fit a polynomial to the input binary image.
Receive:
img: Processed images.
Return:
left_fit, right_fit : It fits the second order polynomial on the left and the right.
left_lane_inds, right_lane_inds : location index of the left and right lane lines.
visualization_data : Save a certain amount of sliding window width and height and
Take a histogram of the bottom half of the image As a tuple.
"""
# Take a histogram of the bottom half of the image
histogram = np.sum(img[img.shape[0]//2:,:], axis=0)
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = np.int(histogram.shape[0]//2)
quarter_point = np.int(midpoint//2)
# Previously the left/right base was the max of the left/right half of the histogram
# this changes it so that only a quarter of the histogram (directly to the left/right) is considered
leftx_base = np.argmax(histogram[quarter_point:midpoint]) + quarter_point
rightx_base = np.argmax(histogram[midpoint:(midpoint+quarter_point)]) + midpoint
# Choose the number of sliding windows
nwindows = 50
# Set height of windows
window_height = np.int(img.shape[0]/nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated for each window
leftx_current = leftx_base
rightx_current = rightx_base
# Set the width of the windows +/- margin
margin = 80
# Set minimum number of pixels found to recenter window
minpix = 40
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Rectangle data for visualization
rectangle_data = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = img.shape[0] - (window+1)*window_height
win_y_high = img.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
rectangle_data.append((win_y_low, win_y_high, win_xleft_low, win_xleft_high, win_xright_low, win_xright_high))
# Identify the nonzero pixels in x and y within the window
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) &
(nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) &
(nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
left_fit, right_fit = (None, None)
# Fit a second order polynomial to each
if len(leftx) != 0:
left_fit = np.polyfit(lefty, leftx, 2)
if len(rightx) != 0:
right_fit = np.polyfit(righty, rightx, 2)
visualization_data = (rectangle_data, histogram)
return left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data
# Visualize the sliding windows over an example test image.
img = test_images_processed[0]
left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data = sliding_window(img)
rectangles = visualization_data[0]
histogram = visualization_data[1]
# Create an output image to draw on and visualize the result
out_img = np.uint8(np.dstack((img, img, img))*255)
# Generate x and y values for plotting
ploty = np.linspace(0, img.shape[0]-1, img.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
for rect in rectangles:
# Draw the windows on the visualization image
cv2.rectangle(out_img,(rect[2],rect[0]),(rect[3],rect[1]),(0,255,0), 2)
cv2.rectangle(out_img,(rect[4],rect[0]),(rect[5],rect[1]),(0,255,0), 2)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [100, 200, 255]
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
plt.subplot(1, 2, 2)
# show the out_img image
plt.imshow(out_img)
plt.xlabel('Sliding window', fontsize=15)
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()
once you know where the lines are in one frame of video, you can do a highly targeted search for them in the next frame. This is equivalent to using a customized region of interest for each frame of video, and should help you track the lanes through sharp curves and tricky conditions.
def polyfit_fit(img, left_fit, right_fit):
"""
Fit a polynomial to the input binary image based upon a previous fit.
Receive Parameters:
img: Processed images.
left_fit, right_fit: It fits the second order polynomial on the left and the right.
Return Parameters:
left_fit_new, right_fit_new: It new fits the second order polynomial on the left and the right.
left_lane_inds, right_lane_inds: location index of the left and right lane lines.
"""
# Identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
margin = 100
left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] - margin)) &
(nonzerox < (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] + margin)))
right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] - margin)) &
(nonzerox < (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] + margin)))
# Again, extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
# Fit a second order polynomial to each
left_fit_new, right_fit_new = (None, None)
if len(leftx) != 0:
left_fit_new = np.polyfit(lefty, leftx, 2)
if len(rightx) != 0:
right_fit_new = np.polyfit(righty, rightx, 2)
return left_fit_new, right_fit_new, left_lane_inds, right_lane_inds
# Visualize the polyfit_prev_fit over the an example image.
margin = 50
img = test_images_processed[0]
left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data = sliding_window(img)
left_fit2, right_fit2, left_lane_inds2, right_lane_inds2 = polyfit_fit(img, left_fit, right_fit)
ploty = np.linspace(0, img.shape[0]-1, img.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
left_fitx2 = left_fit2[0]*ploty**2 + left_fit2[1]*ploty + left_fit2[2]
right_fitx2 = right_fit2[0]*ploty**2 + right_fit2[1]*ploty + right_fit2[2]
# Create an image to draw on and an image to show the selection window
out_img = np.uint8(np.dstack((img, img, img))*255)
window_img = np.zeros_like(out_img)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds2], nonzerox[left_lane_inds2]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds2], nonzerox[right_lane_inds2]] = [0, 0, 255]
# Generate a polygon to illustrate the search window area
# And recast the x and y points into usable format for cv2.fillPoly()
left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, ploty])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, ploty])))])
right_line_pts = np.hstack((right_line_window1, right_line_window2))
# Draw the lane onto the warped blank image
cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
# show the result image
plt.subplot(1, 2, 2)
plt.imshow(result)
plt.xlabel('Polyfit using previous fit', fontsize=15)
plt.plot(left_fitx2, ploty, color='yellow')
plt.plot(right_fitx2, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()
step 1.The radius of curvature at any point x of the function x=f(y) is given as follows:
$$ R_{curve}= \frac{[1+(\frac{{\rm d}x}{{\rm d}y})^2]^{3/2}}{\left| \frac{{\rm d}^2x}{{\rm d}y^2} \right|} $$
In the case of the second order polynomial above, the first and second derivatives are:
$ f'(y)=\frac{{\rm d}x}{{\rm d}y}=2Ay+B $
$ f''(y)=\frac{{\rm d}^2x}{{\rm d}y^2}=2A $
So, our equation for radius of curvature becomes:
$$ R_{curve}= \frac{(1+(2Ay+B)^2)^{3/2}}{\left| {2A} \right|} $$
In the code, the specific equation is as follows:
((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
left_fit_cr[0]: on behalf of A. is the first coefficient (the y-squared coefficient) of the second order polynomial fit.
left_fit_cr[1]: on behalf of B. is the second coefficient.
y_eval*ym_per_pix: on behalf of y. y_eval is the y position within the image upon which the curvature calculation is based. y_meters_per_pixel is the factor used for converting from pixels to meters.
step 2.Then calculate the code of the vehicle in the center of the lane:
l_fit_x_int = l_fit[0]*h**2 + l_fit[1]*h + l_fit[2]
r_fit_x_int = r_fit[0]*h**2 + r_fit[1]*h + r_fit[2]
lane_center_position = (r_fit_x_int + l_fit_x_int) /2
center_dist = (car_position - lane_center_position) * xm_per_pix
l_fit_x_int and r_fit_x_int are the x-intercepts of the left and right fits.The car position is the difference between these intercept points and the image midpoint (assuming that the camera is mounted at the center of the vehicle).
def curv_pos(img, l_fit, r_fit, l_lane_inds, r_lane_inds):
"""
Calculating the lane curvature and the vehicle position on the lane.
Receive Parameters:
img: Processed images.
l_fit, r_fit: It fits the second order polynomial on the left and the right.
l_lane_inds, r_lane_inds: location index of the left and right lane lines.
Return Parameters:
left_curverad : left lane curvature.
right_curverad : right lane curvature.
center_dist : The approximate location of the camera.
"""
# Define conversions in x and y from pixels space to meters
ym_per_pix = 3.048/100 # meters per pixel in y dimension
xm_per_pix = 3.7/378 # meters per pixel in x dimension
left_curverad, right_curverad, center_dist = (0, 0, 0)
# Define y-value where we want radius of curvature
# I'll choose the maximum y-value, corresponding to the bottom of the image
h = img.shape[0]
ploty = np.linspace(0, h-1, h)
y_eval = np.max(ploty)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Again, extract left and right line pixel positions
leftx = nonzerox[l_lane_inds]
lefty = nonzeroy[l_lane_inds]
rightx = nonzerox[r_lane_inds]
righty = nonzeroy[r_lane_inds]
if len(leftx) != 0 and len(rightx) != 0:
# Fit new polynomials to x,y in world space
left_fit_cr = np.polyfit(lefty*ym_per_pix, leftx*xm_per_pix, 2)
right_fit_cr = np.polyfit(righty*ym_per_pix, rightx*xm_per_pix, 2)
# Calculate the new radii of curvature
# Now our radius of curvature is in meters
left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
# Distance from center is image x midpoint - mean of l_fit and r_fit intercepts
if r_fit is not None and l_fit is not None:
car_position = img.shape[1]/2
l_fit_x_int = l_fit[0]*h**2 + l_fit[1]*h + l_fit[2]
r_fit_x_int = r_fit[0]*h**2 + r_fit[1]*h + r_fit[2]
lane_center_position = (r_fit_x_int + l_fit_x_int) /2
center_dist = (car_position - lane_center_position) * xm_per_pix
return left_curverad, right_curverad, center_dist
# Visualize the curv_pos over the an example image.
rad_l, rad_r, d_center = curv_pos(test_images_processed[0], left_fit, right_fit, left_lane_inds2, right_lane_inds2)
print('Radius of curvature for example:', rad_l, 'm,', rad_r, 'm')
print('Distance from lane center for example:', d_center, 'm')
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
plt.subplot(1, 2, 2)
# show the result image
plt.imshow(result)
plt.xlabel('Polyfit using previous fit', fontsize=15)
plt.plot(left_fitx2, ploty, color='yellow')
plt.plot(right_fitx2, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()
def draw_lane(or_img, binary_img, l_fit, r_fit, Minv):
"""
Draw the detected lane over the input image.
Receive Parameters:
or_img: Original input image.
binary_img: Preprocessed image.
l_fit, r_fit: Detected lanes.
Minv: Calibration matrix.
Return Parameters:
result : A picture with a Lane Boundaries.
"""
new_img = np.copy(or_img)
if l_fit is None or r_fit is None:
return or_img
# Create an image to draw the lines on
warp_zero = np.zeros_like(binary_img).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
h,w = binary_img.shape
ploty = np.linspace(0, h-1, num=h)
left_fitx = l_fit[0]*ploty**2 + l_fit[1]*ploty + l_fit[2]
right_fitx = r_fit[0]*ploty**2 + r_fit[1]*ploty + r_fit[2]
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
cv2.polylines(color_warp, np.int32([pts_left]), isClosed=False, color=(255,0,255), thickness=15)
cv2.polylines(color_warp, np.int32([pts_right]), isClosed=False, color=(0,255,255), thickness=15)
newwarp = cv2.warpPerspective(color_warp, Minv, (w, h))
result = cv2.addWeighted(new_img, 1, newwarp, 0.5, 0)
return result
def write_data(or_img, curv_rad, center_dist):
"""
Write the lane curvature and vehicle position over the input image.
Receive Parameters:
or_img: Original input image.
curv_rad: Lane curvature.
center_dist: Vehicle position.
Return new_img:
new_img: Show pictures of related parameters.
"""
new_img = np.copy(or_img)
h = new_img.shape[0]
font = cv2.FONT_HERSHEY_DUPLEX
text = 'Curve radius: ' + '{:04.2f}'.format(curv_rad) + 'm'
cv2.putText(new_img, text, (40,70), font, 1.5, (255,255,255), 2, cv2.LINE_AA)
direction = ''
if center_dist > 0:
direction = 'right'
elif center_dist < 0:
direction = 'left'
abs_center_dist = abs(center_dist)
text = '{:04.3f}'.format(abs_center_dist) + 'm ' + direction + ' of center'
cv2.putText(new_img, text, (40,120), font, 1.5, (255,255,255), 2, cv2.LINE_AA)
return new_img
# Use the first photo of the test set.
or_img_i = test_images_i[0]
pro_img = test_images_processed[0]
# Get two lane lines related data.
left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data = sliding_window(pro_img)
left_fit2, right_fit2, left_lane_inds2, right_lane_inds2 = polyfit_fit(pro_img, left_fit, right_fit)
rad_l, rad_r, d_center = curv_pos(pro_img, left_fit, right_fit, left_lane_inds2, right_lane_inds2)
# draw the lane area
result = draw_lane(or_img_i, pro_img, left_fit, right_fit, Minv)
result = write_data(result, (rad_l+rad_r)/2, d_center)
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
# show the Draw Lane
plt.subplot(1, 2, 2)
plt.imshow(result)
plt.xlabel('Drawing Lane and parameters', fontsize=15)
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()
# Define a class to receive the characteristics of each line detection
class Line():
def __init__(self):
# was the line detected in the last iteration?
self.detected = False
# x values of the last n fits of the line
self.recent_xfitted = []
#average x values of the fitted line over the last n iterations
self.bestx = None
#polynomial coefficients averaged over the last n iterations
self.best_fit = None
#polynomial coefficients for the most recent fit
self.current_fit = []
#radius of curvature of the line in some units
self.radius_of_curvature = None
#distance in meters of vehicle center from the line
self.line_base_pos = None
#difference in fit coefficients between last and new fits
self.diffs = np.array([0,0,0], dtype='float')
#number of detected pixels
self.px_count = None
def add_fit(self, fit, inds):
if fit is not None:
self.detected = True
self.px_count = np.count_nonzero(inds)
self.current_fit.append(fit)
if len(self.current_fit) > 5:
self.current_fit = self.current_fit[len(self.current_fit)-5:]
self.best_fit = np.average(self.current_fit, axis=0)
else:
self.detected = False
if len(self.current_fit) > 0:
self.current_fit = self.current_fit[:len(self.current_fit)-1]
if len(self.current_fit) > 0:
self.best_fit = np.average(self.current_fit, axis=0)
def video_processor(img):
"""
From the input to the picture direct output has processed the completed lane, curvature, vehicle position picture.
Receive Parameters:
img: Input frame.
Return Parameters:
image_output: The output is processed through a series of images.
"""
undist_img = cv2.undistort(img, mtx, dist, None, mtx)
bin2_img, Minv = pipeline_process(img)
if not l_line.detected or not r_line.detected:
l_fit, r_fit, l_lane_inds, r_lane_inds, _ = sliding_window(bin2_img)
else:
l_fit, r_fit, l_lane_inds, r_lane_inds = polyfit_fit(bin2_img, l_line.best_fit, r_line.best_fit)
l_line.add_fit(l_fit, l_lane_inds)
r_line.add_fit(r_fit, r_lane_inds)
img_out = draw_lane(undist_img, bin2_img, l_fit, r_fit, Minv)
rad_l, rad_r, d_center = curv_pos(bin2_img, l_fit, r_fit, l_lane_inds, r_lane_inds)
image_output = write_data(img_out, (rad_l+rad_r)/2, d_center)
return image_output
l_line = Line()
r_line = Line()
project_video_output = 'project_video_output.mp4'
project_video_input = VideoFileClip('project_video.mp4')
processed_video = project_video_input.fl_image(video_processor)
%time processed_video.write_videofile(project_video_output, audio=False)
HTML("""
<video width="960" height="540" controls>
<source src="{0}">
</video>
""".format(project_video_output))